Project Summary

In this section we are going to summarise what we have done for our project and provide all the code, images and videos that we have made. As we have explained in the previous pages (week 1-4), we have designed a robotic arm that consists of four servo motors connected together with 3 pin cables and different shaped frames. Those servos are internally built with pressure and heat sensors to get feedbacks on their state, a potentiometer to know their position, and a little Arduino which can receive commands and execute them.
The main purpose of the robotic arm is to show that it can be manipulated as if it was a real human arm. It mimic human constraints as much as possible.

The robotic arm has been set to have constraints so that it does not execute invalid moves which a human can not do. For the project we have designed a simple housing that allows the arm to hang just like a relaxed human arm and we have dressed it up, so that it looks more fun and realistic. We use a C++ graphical interface using the library Qt, and URBIscript which uses a specific Urbi language to interact with the servo motors. We use the library bioloid with Urbi to make the interaction easier.

During the coding, we have experienced quite a few problems, but all were solved on time. The first problem was to create an interface with Qt which can interact with the Urbi server. However with a bit of research, we have managed to find a simple solution. As Urbi is a server, we have set it up to listen on a certain port. Then with the C++ program, we created a function to send it messages using a socket connection. The initial idea was to use the serial port but it appeared not to be a good idea because Urbi was already using it.

Another problem that we encountered was to stop the record script. The script being executed in the foreground and in an infinite loop, we were not able to stop it. However again with a bit of research we found some functions which allow us to execute it in background and so to execute commands to stop it.

The last problem we had was to take in account the constraints we fixed to respect a human arm. In fact, the recorder did not care about them and recorded any given movement. We finally found the solution by controlling whether a movement should be played while reading the recorded positions. Only the position inside the boundaries are sent to the servo motors. The others are ignored.

To interact with the robot, we use two analog inputs, namely a mouse and a keyboard. Obviously, we can use any controller connected to the computer, like a joystick or a trackball.
The mouse allows the user to interact with the graphical interface and press buttons that we have designed to send specific messages to Urbi which react to them by either playing already recorded scripts, playing a demo, reset the servos of the robotic arm or record your own script and then play it straight away. The keyboard was used to manually control the robotic arm. It allows the user to press different keys to trigger each servo individually and try out different kinds of movements without any recorded script.

There are two different outputs in this project: the motors and the screen. The motors act according to an order sent by Urbi. They make the arm move and allow a physical output of the commands executed. On another side, the user might want to control the internal state of the program. That is why we also display some messages on the server terminal so that an output of what is happening is accessible. A simple user is not supposed to interact with the robot outside of the graphical interface. But an experimented user or the owner of the robot might need it.

Below are the pictures that we have taken for the project along with the presentation video and all the fully explained and commented code that we have written.



















Following is the code used to create the graphical interface. You can also download it from the links below:
Urbi files
C++ files

main.cpp
#include <QtGui/QApplication>
#include "mainwindow.h"

int main(int argc, char *argv[])
{
QApplication a(argc, argv);
MainWindow w;
w.show();

return a.exec();
}

mainWindow.h
#ifndef MAINWINDOW_H
#define MAINWINDOW_H

#include <QMainWindow>
#include <QKeyEvent>
#include "socketConnection.hpp"

namespace Ui {
class MainWindow;
}

class MainWindow : public QMainWindow
{
Q_OBJECT

public:
explicit MainWindow(QWidget *parent = 0);
virtual void keyPressEvent ( QKeyEvent * event );
~MainWindow();

private slots:
void on_reset_clicked();

void on_demo_clicked();

void on_record_clicked();

void on_play_clicked();

void on_play1_clicked();

void on_play2_clicked();

void on_play3_clicked();

private:
Ui::MainWindow *ui;
bool stopRecording;
};

#endif MAINWINDOW_H

mainWindow.cpp
#include "mainwindow.h"
#include "ui_mainwindow.h"

MainWindow::MainWindow(QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainWindow)
{
ui->setupUi(this);
stopRecording = false;
}

MainWindow::~MainWindow()
{
delete ui;
}

void MainWindow::keyPressEvent(QKeyEvent *event)
{
switch(event->key()){
case (Qt::Key_S):
socketConnection("s1_move_down");
break;

case(Qt::Key_Z):
socketConnection("s1_move_up");
break;

case (Qt::Key_Q):
socketConnection("s2_move_down");
break;

case(Qt::Key_D):
socketConnection("s2_move_up");
break;

case (Qt::Key_I):
socketConnection("s3_move_up");
break;

case(Qt::Key_K):
socketConnection("s3_move_down");
break;

case (Qt::Key_J):
socketConnection("e_move_down");
break;

case(Qt::Key_L):
socketConnection("e_move_up");
break;

case(Qt::Key_Escape):
close();
break;

case(Qt::Key_W):
socketConnection("stopRecording");
break;
}
}

void MainWindow::on_reset_clicked()
{
socketConnection("reset");
}

void MainWindow::on_demo_clicked()
{
socketConnection("demo");
}

void MainWindow::on_play_clicked()
{
socketConnection("play");
}

void MainWindow::on_record_clicked()
{
socketConnection("record");
}

void MainWindow::on_play1_clicked()
{
socketConnection("play1");
}

void MainWindow::on_play2_clicked()
{
socketConnection("play2");
}

void MainWindow::on_play3_clicked()
{
socketConnection("play3");
}

socketConnection.hpp
#ifndef SOCKETCONNECTION_HPP
#define SOCKETCONNECTION_HPP

#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <string>


#define INVALID_SOCKET -1
#define SOCKET_ERROR -1
#define closesocket(s) close(s)
#define PORT 5353

typedef int SOCKET;
typedef struct sockaddr_in SOCKADDR_IN;
typedef struct sockaddr SOCKADDR;


using namespace std;

class socketConnection
{
private:
SOCKET sock;
SOCKADDR_IN sin;
int erreur;
bool connected;
public:
socketConnection(string message);
};

#endif SOCKETCONNECTION_HPP

socketConnection.cpp
#include "socketConnection.hpp"

socketConnection::socketConnection(string message){
int erreur = 0;

SOCKET sock;
SOCKADDR_IN sin;

if(!erreur)
{
/* socket creation */
sock = socket(AF_INET, SOCK_STREAM, 0);

/* connection config */
sin.sin_addr.s_addr = inet_addr("127.0.0.1");
sin.sin_family = AF_INET;
sin.sin_port = htons(PORT);

/* if the program can connect */
if(connect(sock, (SOCKADDR*)&sin, sizeof(sin)) != SOCKET_ERROR){
printf("Connected on server %s on port %d\n", inet_ntoa(sin.sin_addr), htons(sin.sin_port));
cout << message.length();
send(sock, message.c_str(), message.length(),0);
}
else
printf("Connection refused\n");

/*close the socket*/
closesocket(sock);
}
}

mainWindow.ui
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>MainWindow</class>
<widget class="QMainWindow" name="MainWindow">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>544</width>
<height>418</height>
</rect>
</property>
<property name="windowTitle">
<string>MainWindow</string>
</property>
<widget class="QWidget" name="centralWidget">
<widget class="QWidget" name="gridLayoutWidget">
<property name="geometry">
<rect>
<x>9</x>
<y>9</y>
<width>531</width>
<height>353</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout">
<item row="1" column="3" rowspan="13">
<widget class="Line" name="line">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
</widget>
</item>
<item row="8" column="5">
<widget class="QSlider" name="shoulder3">
<property name="enabled">
<bool>false</bool>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="13" column="5">
<widget class="QSlider" name="elbow">
<property name="enabled">
<bool>false</bool>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="1" column="2">
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="4" column="4">
<widget class="QLabel" name="label_2">
<property name="text">
<string>Shoulder 2</string>
</property>
</widget>
</item>
<item row="12" column="4">
<widget class="QLabel" name="label_4">
<property name="text">
<string>Elbow</string>
</property>
</widget>
</item>
<item row="5" column="5">
<widget class="QSlider" name="shoulder2">
<property name="enabled">
<bool>false</bool>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="7" column="4">
<widget class="QLabel" name="label_3">
<property name="text">
<string>Shoulder 3</string>
</property>
</widget>
</item>
<item row="1" column="4">
<widget class="QLabel" name="label">
<property name="text">
<string>Shoulder 1</string>
</property>
</widget>
</item>
<item row="2" column="6">
<spacer name="horizontalSpacer_3">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Minimum</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="2" column="4">
<spacer name="horizontalSpacer_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="1" column="1">
<widget class="QPushButton" name="play1">
<property name="cursor">
<cursorShape>PointingHandCursor</cursorShape>
</property>
<property name="text">
<string>Play script 1</string>
</property>
</widget>
</item>
<item row="11" column="4" colspan="2">
<widget class="Line" name="line_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="6" column="4" colspan="2">
<widget class="Line" name="line_3">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="3" column="4" colspan="2">
<widget class="Line" name="line_4">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="2" column="5">
<widget class="QSlider" name="shoulder1">
<property name="enabled">
<bool>false</bool>
</property>
<property name="minimum">
<number>-200</number>
</property>
<property name="maximum">
<number>160</number>
</property>
<property name="singleStep">
<number>10</number>
</property>
<property name="value">
<number>-147</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="1" column="0">
<spacer name="horizontalSpacer_4">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="2" column="1">
<widget class="QPushButton" name="play2">
<property name="cursor">
<cursorShape>PointingHandCursor</cursorShape>
</property>
<property name="text">
<string>Play script 2</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QPushButton" name="play3">
<property name="cursor">
<cursorShape>PointingHandCursor</cursorShape>
</property>
<property name="text">
<string>Play script 3</string>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QPushButton" name="record">
<property name="cursor">
<cursorShape>PointingHandCursor</cursorShape>
</property>
<property name="text">
<string>Record</string>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QPushButton" name="play">
<property name="cursor">
<cursorShape>PointingHandCursor</cursorShape>
</property>
<property name="text">
<string>Play</string>
</property>
</widget>
</item>
<item row="5" column="1">
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
<item row="9" column="1">
<spacer name="verticalSpacer_2">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
<item row="12" column="1">
<widget class="QPushButton" name="demo">
<property name="cursor">
<cursorShape>PointingHandCursor</cursorShape>
</property>
<property name="text">
<string>Demo</string>
</property>
</widget>
</item>
<item row="13" column="1">
<widget class="QPushButton" name="reset">
<property name="cursor">
<cursorShape>PointingHandCursor</cursorShape>
</property>
<property name="text">
<string>Reset</string>
</property>
</widget>
</item>
</layout>
</widget>
</widget>
<widget class="QMenuBar" name="menuBar">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>544</width>
<height>25</height>
</rect>
</property>
</widget>
<widget class="QToolBar" name="mainToolBar">
<attribute name="toolBarArea">
<enum>TopToolBarArea</enum>
</attribute>
<attribute name="toolBarBreak">
<bool>false</bool>
</attribute>
</widget>
<widget class="QStatusBar" name="statusBar"/>
</widget>
<layoutdefault spacing="6" margin="11"/>
<resources/>
<connections/>
</ui>

And this is the urbi code.

start.u
load("roboArm.u");
var arm = roboArm.new;

roboArm.u
load("dynamixel.u");
var Global.d = Dynamixel.new;
var range = 0.1;

var server = Server.new;
at (server.connection?(var socket))
at (socket.received?(var data)){
switch (data)
{
case "reset":
echo ("Reset");
arm.reset;
case "demo":
echo ("Demo");
arm.sM;
case "record":
arm.stopRecording = false;
echo ("Recording");
arm.record;
case "stopRecording":
arm.stopRecording = true;
arm.record;
echo ("Record stopped");
case "play":
echo ("Playing");
arm.player;
case "play1":
echo ("Playing");
arm.player1;
case "play2":
echo ("Playing");
arm.player2;
case "play3":
echo ("Playing");
arm.player3;
case "s1_move_up":
if(arm.shoulder1.position+range <= arm.shoulder1.upperPos){
echo ("Move up");
arm.shoulder1.targetPos = arm.shoulder1.position+range;
}
case "s1_move_down":
if(arm.shoulder1.position-range >= arm.shoulder1.lowerPos){
echo ("Move Down");
arm.shoulder1.targetPos = arm.shoulder1.position-range;
}
case "s2_move_up":
if(arm.shoulder2.position+range <= arm.shoulder2.iniPos){
echo ("Move up");
arm.shoulder2.targetPos = arm.shoulder2.position+range;
}
case "s2_move_down":
if(arm.shoulder2.position-range >= arm.shoulder2.lowerPos){
echo ("Move Down");
arm.shoulder2.targetPos = arm.shoulder2.position-range;
}
case "s3_move_up":
if(arm.shoulderRot.position+range <= arm.shoulderRot.maxRight){
echo ("Move up");
arm.shoulderRot.targetPos = arm.shoulderRot.position+range;
}
case "s3_move_down":
if(arm.shoulderRot.position-range >= arm.shoulderRot.maxLeft){
echo ("Move Down");
arm.shoulderRot.targetPos = arm.shoulderRot.position-range;
}
case "e_move_up":
if(arm.elbow.position+range <= arm.elbow.iniPos){
echo ("Move up");
arm.elbow.targetPos = arm.elbow.position+range;
}
case "e_move_down":
if(arm.elbow.position-range >= arm.elbow.lowerPos){
echo ("Move Down");
arm.elbow.targetPos = arm.elbow.position-range;
}
};
};

server.listen("localhost", "5353");

class roboArm
{
function init()
{
var speed = 2;


var this.stopRecording = false;


var this.shoulder1 = Dynamixel.Device.new(d,17,Dynamixel.DeviceDesc.AX12);
var this.shoulder2 = Dynamixel.Device.new(d,9,Dynamixel.DeviceDesc.AX12);
var this.shoulderRot = Dynamixel.Device.new(d,2,Dynamixel.DeviceDesc.AX12);
var this.elbow = Dynamixel.Device.new(d,6,Dynamixel.DeviceDesc.AX12);


this.shoulder1.targetSpeed = speed;
this.shoulder2.targetSpeed = speed;
this.shoulderRot.targetSpeed = speed;
this.elbow.targetSpeed = speed;


var this.shoulder1.iniPos = -1.47;
var this.shoulder1.upperPos = 1.6;
var this.shoulder1.lowerPos = -2;


var this.shoulder2.iniPos = 1.27;
var this.shoulder2.lowerPos = -1.21;


var this.shoulderRot.iniPos = -0.03;
var this.shoulderRot.maxLeft = -1.45;
var this.shoulderRot.maxRight = 0.57;


var this.elbow.iniPos = 1.58;
var this.elbow.lowerPos = -0.57;
};


function reset()
{
var delay = 1;
this.shoulder1.targetPos = this.shoulder1.iniPos;
sleep(delay);
this.shoulder2.targetPos = this.shoulder2.iniPos;
sleep(delay);
this.shoulderRot.targetPos = this.shoulderRot.iniPos;
sleep(delay);
this.elbow.targetPos = this.elbow.iniPos;
sleep(delay);
unlock();
};


function unlock(){
this.shoulderRot.load = 0;
this.elbow.load = 0;
this.shoulder2.load = 0;
this.shoulder1.load = 0;
};


function sM()
{
var delay = 0.5;
var range = 0.05;
reset();
sleep(delay);
while(this.shoulder1.position<this.shoulder1.upperPos-range)
this.shoulder1.targetPos = this.shoulder1.upperPos;
sleep(delay);
while(this.shoulder1.position>this.shoulder1.lowerPos+range)
this.shoulder1.targetPos = this.shoulder1.lowerPos;
sleep(delay);
while(this.shoulder1.position<this.shoulder1.iniPos-range)
this.shoulder1.targetPos = this.shoulder1.iniPos;
sleep(delay);


while(this.shoulder2.position>this.shoulder2.lowerPos+range)
this.shoulder2.targetPos = this.shoulder2.lowerPos;
sleep(delay);
while(this.shoulder2.position<this.shoulder2.iniPos-range)
this.shoulder2.targetPos = this.shoulder2.iniPos;
sleep(delay);


while(this.shoulderRot.position>this.shoulderRot.maxLeft+range)
this.shoulderRot.targetPos = this.shoulderRot.maxLeft;
sleep(delay);
while(this.shoulderRot.position<this.shoulderRot.maxRight-range)
this.shoulderRot.targetPos = this.shoulderRot.maxRight;
sleep(delay);
while(this.shoulderRot.position>this.shoulderRot.iniPos+range)
this.shoulderRot.targetPos = this.shoulderRot.iniPos;
sleep(delay);


while(this.elbow.position>this.elbow.lowerPos+range)
this.elbow.targetPos = this.elbow.lowerPos;
sleep(delay);
while(this.elbow.position<this.elbow.iniPos-range)
this.elbow.targetPos = this.elbow.iniPos;
sleep(delay);


reset();
};


function simpleMovement2()
{
var delay = 0.1;
this.shoulder1.targetPos = this.shoulder1.upperPos;
sleep(delay);
this.shoulder2.targetPos = this.shoulder2.iniPos;
};


function simpleMovement3()
{
var delay = 0.1;
this.shoulder1.targetPos = this.shoulder1.upperPos;
sleep(delay);
this.shoulderRot.targetPos = this.shoulderRot.maxLeft;
sleep(delay);
this.elbow.targetPos = this.elbow.lowerPos;
};


function player(){
reset;
sleep(1s);

var is = InputStream.new(File.new("output.txt"));

var line;

reading:every(0.01) {
line = is.getLine.acceptVoid;
if (line.isVoid) {
reading.stop();
};
echo(line);
var ns = line.split(",");
if(ns[0].asFloat()<=this.shoulder1.upperPos && ns[0].asFloat()>=this.shoulder1.lowerPos)
this.shoulder1.targetPos = ns[0].asFloat();


if(ns[1].asFloat()<=this.shoulder2.iniPos && ns[1].asFloat()>=this.shoulder2.lowerPos)
this.shoulder2.targetPos = ns[1].asFloat();


if(ns[2].asFloat()<=this.shoulderRot.maxRight && ns[2].asFloat()>=this.shoulderRot.maxLeft)
this.shoulderRot.targetPos = ns[2].asFloat();


if(ns[3].asFloat()<=this.elbow.iniPos && ns[3].asFloat()>=this.elbow.lowerPos)
this.elbow.targetPos = ns[3].asFloat();
};
};


function player1(){
reset;
sleep(1s);

var is = InputStream.new(File.new("output1.txt"));

var line;

reading:every(0.01) {
line = is.getLine.acceptVoid;
if (line.isVoid) {
reading.stop();
};
echo(line);
var ns = line.split(",");
if(ns[0].asFloat()<=this.shoulder1.upperPos && ns[0].asFloat()>=this.shoulder1.lowerPos)
this.shoulder1.targetPos = ns[0].asFloat();


if(ns[1].asFloat()<=this.shoulder2.iniPos && ns[1].asFloat()>=this.shoulder2.lowerPos)
this.shoulder2.targetPos = ns[1].asFloat();


if(ns[2].asFloat()<=this.shoulderRot.maxRight && ns[2].asFloat()>=this.shoulderRot.maxLeft)
this.shoulderRot.targetPos = ns[2].asFloat();


if(ns[3].asFloat()<=this.elbow.iniPos && ns[3].asFloat()>=this.elbow.lowerPos)
this.elbow.targetPos = ns[3].asFloat();
};
};


function player2(){
reset;
sleep(1s);

var is = InputStream.new(File.new("output2.txt"));

var line;

reading:every(0.01) {
line = is.getLine.acceptVoid;
if (line.isVoid) {
reading.stop();
};
echo(line);
var ns = line.split(",");
if(ns[0].asFloat()<=this.shoulder1.upperPos && ns[0].asFloat()>=this.shoulder1.lowerPos)
this.shoulder1.targetPos = ns[0].asFloat();


if(ns[1].asFloat()<=this.shoulder2.iniPos && ns[1].asFloat()>=this.shoulder2.lowerPos)
this.shoulder2.targetPos = ns[1].asFloat();


if(ns[2].asFloat()<=this.shoulderRot.maxRight && ns[2].asFloat()>=this.shoulderRot.maxLeft)
this.shoulderRot.targetPos = ns[2].asFloat();


if(ns[3].asFloat()<=this.elbow.iniPos && ns[3].asFloat()>=this.elbow.lowerPos)
this.elbow.targetPos = ns[3].asFloat();
};
};


function player3(){
reset;
sleep(1s);

var is = InputStream.new(File.new("output3.txt"));

var line;

reading:every(0.01) {
line = is.getLine.acceptVoid;
if (line.isVoid) {
reading.stop();
};
echo(line);
var ns = line.split(",");
if(ns[0].asFloat()<=this.shoulder1.upperPos && ns[0].asFloat()>=this.shoulder1.lowerPos)
this.shoulder1.targetPos = ns[0].asFloat();


if(ns[1].asFloat()<=this.shoulder2.iniPos && ns[1].asFloat()>=this.shoulder2.lowerPos)
this.shoulder2.targetPos = ns[1].asFloat();


if(ns[2].asFloat()<=this.shoulderRot.maxRight && ns[2].asFloat()>=this.shoulderRot.maxLeft)
this.shoulderRot.targetPos = ns[2].asFloat();


if(ns[3].asFloat()<=this.elbow.iniPos && ns[3].asFloat()>=this.elbow.lowerPos)
this.elbow.targetPos = ns[3].asFloat();
};
};


function record(){
if(!stopRecording){
reset();
var o1 = OutputStream.new(File.create("output.txt"));
detach({
writing:every(0.01){
o1 << this.shoulder1.position << "," << this.shoulder2.position << "," << this.shoulderRot.position << "," << this.elbow.position << "\n";
};
});
}
else{
writing.stop;
}
};


function record2(){
reset();
var o1 = OutputStream.new(File.create("output.txt"));
every(0.03) {
o1 << this.shoulder1.position << "," << this.shoulder2.position << "," << this.shoulderRot.position << "," << this.elbow.position << "\n";
},
};
};

dynamixel.u
/*

• Copyright (C) 2010, Gostai S.A.S.

*
* This software is provided "as is" without warranty of any kind,

• either expressed or implied, including but not limited to the
• implied warranties of fitness for a particular purpose.

*
* See the LICENSE file for more information.
*/


if (!hasSlot("DynamixelPacket"))
loadModule("/home/thibbo/urbi-for-bioloid/bioloid.so");
/** Link to a bioloid CM-5 brick patched with gostai firmware.
*

• This code will also work with a direct connection to the dynamixel bus
• (using for instance the usb2dynamixel), if you disable all 'multiRead'
• optimization.

*
* usage:

• var Global.d = Dynamixel.new;
• var m = Dynamixel.Device.new(d, 3, Dynamixel.DeviceDesc.AX12);
• m.val = 100;
• var s = Dynamixel.Device.new(d, 100, Dynamixel.DeviceDesc.AXS1);
• s.buzzerIndex = 10;

*/

class Global.Dynamixel
{
function init(host="/dev/ttyUSB0", speed=57600, cm5 = false)
{
var this.io = IoService.new;
var this.socket = io.makeSocket;
var this.data = "";
var this.m = Mutex.new;
/ Are we connected throug a cm5 or directly on the bus.
var this.cm5 = cm5;
/ Mutex used to prevent multiple parallel operations.
var this.sendMutex = Mutex.new;
var this.reply = Event.new;
var this.lastDataSize = 1;
var this.parseTime = 1;
var this.writeTime = 1;
var this.debug = Tag.new;
/ For each write offset, pairs of (deviceId, value)
var this.writeCache = [[#]] * 256;
/ Time of last read for given offset.
var this.readTime = [time] * 256;
/ Cache of read values.
var this.readValues = 256.seq.map(function(x) {[0] * 256});
/ List of active device IDs
var this.addresses = [];

socket.connectSerial(host, speed, false);
at(socket.error?)
wall("socket error");
};

Write the list of (address, val) in addrVal to index 'idx'.
function multiWrite(addrVal, idx, size)
{
var addresses = addrVal.map(function(x) { x[0]})|
var vals = addrVal.map(function(x) { x[1]});
No reply to multiwrite packets
sendMutex:
socket.syncWrite(DynamixelPacket.multiWrite(addresses, idx,size, vals))
};

Get one packet
function syncGetPacket(dataSize=1)
{
var res|
var p|
var clen|
do(socket)
{
p = ""|
while| (p != "\xFF")
p = read(1)|
p += read(3)|
if (p[1] != "\xff")
{
wall("fail: " + p.asPrintable)|
return nil|
}|
clen = p[3].toAscii|
p += read(clen)|
res = DynamixelPacket.parse(p, dataSize)|
}|
res
};

Write to one motor.
function write(address, idx, size, val)
{
sendMutex: {
socket.syncWrite(DynamixelPacket.write(address, idx, size, val))|
if (idx != 254)
syncGetPacket
}
};

function read(address, idx, size)
{
sendMutex: {
if (address.isA(List))
socket.syncWrite(DynamixelPacket.multiRead(address, idx, size))
else
socket.syncWrite(DynamixelPacket.read(address, idx, size))|
var v = syncGetPacket(size)|
var res = {
if (v.isNil)
nil
else
if (v.size>2 && (address.isA(List) || v[0] == address))
{
if (!address.isA(List))
v[2]
else
{
v.removeFront|
v.removeFront|
v
}
}
else
nil
}|
res
}
};

/ Return the list of existing devices, with their type
function scan(maxid=253, verbose=true, fast=true)
{
var res = [];
debug.block;
var vals = [];
if (verbose)
wall("Scanning for devices, this can take up to 30 seconds.");
if (fast && cm5)
{
var alls = maxid.seq;
Big requests can take time
var chunk = 16;
for(var i: maxid/chunk) if (i*chunk <=maxid) {
vals += read(alls.range(i*chunk, (i*chunk+chunk).min(maxid)), 0, 2)|
}
}|
wall("done")|
for|(var i:maxid)
{
var v = {
if (!fast) read(i, 0, 2)
else
vals[i]
};
switch(v)
{
case 13:
res << (i, DeviceDesc.AXS1)|
if (verbose) wall("Found AXS1 at address " + i)
case 12:
res << (i, DeviceDesc.AX12)|
if (verbose) wall("Found AX12 at address " + i)
case 65535:
case (var x) if (x.isA(Float)):
res << (i, nil)
}
}|
wall("Scanning finished, " + res.size + " device(s) found.");
debug.unblock;
res
};

/** Create one device per scan result, using basename and the index.

• Also create an array with basename.
• Example: [(1, AX12)] will create motor1, motor[1].

*/
function instanciate(scanResult, where=Global)
{
// Extract model list.
var d = Dictionary.new;
for|(var s: scanResult)
d[s[1].basename] = 1;
for|(var k: d.keys)
where.setSlot(k+"s", [nil]*254);
for|(var s: scanResult)
{
var n = s[1].basename + s[0];
wall("Creating " + n);
var d = Device.new(this, s[0], s[1]);
where.setSlot(n, d);
where.getSlot(s[1].basename+"s")[s[0]] = d;
}
}
};

/** Device descriptions.
*

• Each device description is made of:
• -slots: list of (name, size, (readable, writable))
• Must be in the order of addresses, starting from 0, without gaps.
• -multiWrite: list of names for which to use optimized writing.
• -multiRead: list of names for which to use optimized reading.
• -pairs: dictionary targetSlot => (readslot, writeslot)
• create new slots merging tho two read/write ones
• - mapOut[]: Array of functions mapping from user values to dynamixel values
• - mapIn[addr]: Mapping from dynamixel values to user values.
• - range[addr]: (rangemin, rangemax) for value, or nil for none

*
* Only set multiWrite/Read for slots that will be read/written often.
*/
class Dynamixel.DeviceDesc {
function identity(x) { x};
function fallback { call.message};
var mapIn = [getSlot("identity")]*255;
var mapOut = [getSlot("identity")]*255;
var range = [nil] * 255;
function deviceSetup() {};
var R = (true, false);
var W = (false, true);
var RW = (true, true);
var NOOP = (false, false);
var base = [
key size read/write
(model, 2, R),
("version", 1, R),
(ID, 1, RW),
(bitRate, 1, RW),
(returnDelay,1, RW)];
class AX12: this
{
var slots = base + [
key size read/write
(cwLimit, 2, RW),
(ccwLimit, 2, RW),
(reserved, 1, NOOP),
(tempLimit, 1, RW),
(voltageMin, 1, RW),
(voltageMax, 1, RW),
(torqueMax, 2, RW),
(statusLevel,1, RW),
(alarmLED, 1, RW),
(alarmShutdown, 1, RW),
(reserved2, 1, NOOP),
(downCalib, 2, R),
(upCalib, 2, R),
("load", 1, RW),
(led, 1, RW),
(cwMargin, 1, RW),
(ccwMargin, 1, RW),
(cwSlope, 1, RW),
(ccwSlope, 1, RW),
(targetPos, 2, RW),
(targetSpeed,2, RW),
(torqueLimit,2, RW),
(position, 2, R),
(currentSpeed, 2, R),
(torque, 2, R),
(voltage, 1, R),
(temperature,1, R),
(registeredInstruction, 1, R),
(reserved3, 1, NOOP),
(moving, 1, R),
(lock, 1, RW),
(punch, 2, RW)
];
Trigger cow.
this.mapIn = mapIn.clone;
this.mapOut = mapOut.clone;
this.range = range.clone;
Motor position: 0-1023 -> -150-150deg
mapIn[6] = mapIn[8] = mapIn[36]
= function(x) { (x / 1023 - 0.5) * 300deg};
mapOut[6] = mapOut[8] = mapOut[30]
= function(x) { ((x / 300deg) + 0.5) * 1023};
range[6] = range[8] = range[36] = range[30] = (-150deg, 150deg);
Same thing for angle limit
Motor speed: 1023 = 114 RPM
0.0116697 = 1 / 1023 * 228 * pi / 60
mapIn[38] = function(x) { {if (x<1024) x else 1024 - x} * 0.0116697};
mapOut[32] = function(x) { var v = x/0.0116697| if (v<0) 1024-v else v };
range[38] = range[32] = (-228*pi/60, 228*pi/60);
var multiWrite = ["targetPos", "targetSpeed"];
var multiRead = ["position, speed, torque"];
var pairs = [
"val" => ("position", "targetPos"),
"speed" => ("currentSpeed", "targetSpeed")
];
var basename = "motor";
};
class AXS1: this {
var slots = base + [
(reserved5, 5, NOOP),
(tempLimit, 1, RW),
(voltageMin, 1, RW),
(voltageMax, 1, RW),
(reserved2, 2, NOOP),
(statusLevel,1, RW),
(reserved3, 3, NOOP),
(obstacleRange, 1, RW),
(lightRange, 1, RW),
(reserved4, 4, NOOP),
(IRLeft, 1, R),
(IRCenter, 1, R),
(IRRight, 1, R),
(lightLeft, 1, R),
(lightCenter,1, R),
(lightRight, 1, R),
(obstacle, 1, R),
(light, 1, R),
(reserved1, 1, R),
(soundVolume, 1, RW),
(soundVolumeMax, 1, RW),
(clapCount, 1, RW),
(clapTime, 2, RW),
(buzzerIndex,1, RW),
(buzzerTime, 1, RW),
(voltage, 1, R),
(temperature,1, R),
(registeredInstruction, 1, R),
(reserved1, 1, NOOP),
(IRData, 1, R),
(lock, 1, RW),
(IRRX0, 1, R),
(IRRX1, 1, R),
(IRTX0, 1, RW),
(IRTX1, 1, RW),
(obstacleCompare, 1, RW),
(lightCompare, 1, RW)
];
this.mapIn = mapIn.clone;
this.mapOut = mapOut.clone;
this.range = range.clone;
var multiWrite = [];
var multiRead = [];
var pairs = [];
var basename = "sensor";
}
};

class Dynamixel.Device {
function init(conn, address, model)
{
var this.conn = conn;
var this.address = address;
var this.deviceModel = model;
var this.handle = WeakPointer.new;
conn.addresses << address;
createSlots;
};
function createSlots()
{
var idx = 0;
for|(var item: deviceModel.slots)
{
(var name, var size, (var read, var write)) = item|
var myidx = idx |
var writeCache = conn.writeCache[myidx]|
idx += size|
if (!read && !write)
continue|
var v = UVar.new(this, name)|
v.owned = true|
if (read)
v.notifyAccess(handle, {
No multiread primivite without cm5.
if (item not in deviceModel.multiRead || conn.cm5)
closure()
{
var mi = deviceModel.mapIn[myidx] |
var raw = conn.read(address, myidx, size)|
if (!raw.isNil)
v.writeOwned(mi(raw))
}
else
closure()
{
var mi = deviceModel.mapIn[myidx] |
if (time - conn.readTime[myidx] < period)
v.writeOwned(mi(conn.readValues[myidx][address]))
else
{
conn.readTime[myidx] = time|
var vals = conn.read(conn.addresses, myidx, size)|
if (vals.isA(List)
&& vals.size == conn.addresses.size)
{
for|(var i: vals.size)
conn.readValues[myidx][conn.addresses[i]] =
vals[i]|
v.writeOwned(mi(conn.readValues[myidx][address]))
}
else
debug: wall("reply error: " +vals)
}
}}
)|
if (write)
v.notifyChangeOwned(handle, {
if (item not in deviceModel.multiWrite)
closure()
{
var mo = deviceModel.mapOut[myidx]|
conn.write(address, myidx, size, mo(v.val))
}
else
closure()
{
var mo = deviceModel.mapOut[myidx]|
var first = writeCache.empty |
writeCache << (address, mo(v.val))|
if (first)
detach({ sleep(period) |
conn.multiWrite(writeCache, myidx, size) |
writeCache.clear
})
}
})|
if (deviceModel.range[myidx])
{
setProperty(name, "rangemin", deviceModel.range[myidx][0])|
setProperty(name, "rangemax", deviceModel.range[myidx][1])
}
};
for|(var i: deviceModel.pairs)
{
var name = i[0];
var readName = i[1][0];
var writeName = i[1][1];
var v = UVar.new(this, name);
v.owned = true;
v.notifyChangeOwned(handle, closure() { updateSlot(writeName, v.val)});
v.notifyAccess(handle, closure() {v.writeOwned(getSlot(readName).accessor)});
if (! getProperty(writeName, "rangemin").isVoid)
{
setProperty(name, "rangemin", getProperty(writeName, "rangemin"))|
setProperty(name, "rangemax", getProperty(writeName, "rangemax"))
}
}
}
};

trace: trace.block;